//
// Created by ljn on 20-5-27.
//
#include <algorithm>
#include <limits>
#include "glog/logging.h"
#include "PiecewiseJerkSpeedNonlinearIpoptInterface.hpp"

PiecewiseJerkSpeedNonlinearIpoptInterface::
    PiecewiseJerkSpeedNonlinearIpoptInterface(
        const double s_init, const double s_dot_init, const double s_ddot_init,
        const double delta_t, const int num_of_points, const double s_max,
        const double s_dot_max, const double s_ddot_min,
        const double s_ddot_max, const double s_dddot_min,
        const double s_dddot_max)
    : curvature_curve_(0.0, 0.0, 0.0),
      v_bound_func_(0.0, 0.0, 0.0),
      s_init_(s_init),
      s_dot_init_(s_dot_init),
      s_ddot_init_(s_ddot_init),
      delta_t_(delta_t),
      num_of_points_(num_of_points),
      s_max_(s_max),
      s_dot_max_(s_dot_max),
      s_ddot_min_(-std::abs(s_ddot_min)),
      s_ddot_max_(s_ddot_max),
      s_dddot_min_(-std::abs(s_dddot_min)),
      s_dddot_max_(s_dddot_max),
      v_offset_(num_of_points),
      a_offset_(num_of_points * 2) {}

// returns the size of the problem
// 重写了/usr/local/include/coin/IpTNLP.hpp
bool PiecewiseJerkSpeedNonlinearIpoptInterface::get_nlp_info(
    int &n, int &m, int &nnz_jac_g, int &nnz_h_lag,
    IndexStyleEnum &index_style)
{
    num_of_variables_ = num_of_points_ * 3;

    if (use_soft_safety_bound_)
    {
        // 补充
        // complementary slack variable for soft upper and lower s bound
        num_of_variables_ += num_of_points_ * 2;
        lower_s_slack_offset_ = num_of_points_ * 3;
        upper_s_slack_offset_ = num_of_points_ * 4;
    }

    // number of variables is given in constructor
    n = num_of_variables_;

    // 单调  // s monotone constraints s_i+1 - s_i >= 0.0
    num_of_constraints_ = num_of_points_ - 1;

    // jerk bound constraint
    // |s_ddot_i+1 - s_ddot_i| / delta_t <= s_dddot_max
    num_of_constraints_ += num_of_points_ - 1;

    // position equality constraints
    // s_i+1 - s_i - s_dot_i * delta_t - 1/3 * s_ddot_i * delta_t^2 - 1/6 *
    // s_ddot_i+1 * delta_t^2
    num_of_constraints_ += num_of_points_ - 1;

    // velocity equality constraints
    // s_dot_i+1 - s_dot_i - 0.5 * delta_t * s_ddot_i - 0.5 * delta_t * s_ddot_i+1
    num_of_constraints_ += num_of_points_ - 1;

    if (use_v_bound_)
    {
        // speed limit constraints
        // s_dot_i - v_bound_func_(s_i) <= 0.0
        num_of_constraints_ += num_of_points_;
    }

    if (use_soft_safety_bound_)
    {
        // soft safety boundary constraints
        // s_i - soft_lower_s_i + lower_slack_i >= 0.0
        num_of_constraints_ += num_of_points_;

        // s_i - soft_upper_s_i - upper_slack_i <= 0.0
        num_of_constraints_ += num_of_points_;
    }

    // we have m constraints
    m = num_of_constraints_;

    // each constraint's nonzeros number
    nnz_jac_g = 0;
    // s_i+1 - s_i
    nnz_jac_g += (num_of_points_ - 1) * 2;
    // (s_ddot_i+1 - s_ddot_i) / delta_t
    nnz_jac_g += (num_of_points_ - 1) * 2;
    // s_i+1 - s_i - s_dot_i * delta_t - 1/3 * s_ddot_i * delta_t^2 - 1/6 *
    // s_ddot_i+1 * delta_t^2
    nnz_jac_g += (num_of_points_ - 1) * 5;
    // s_dot_i+1 - s_dot_i - 0.5 * s_ddot_i * delta_t - 0.5 * s_ddot_i+1 * delta_t
    nnz_jac_g += (num_of_points_ - 1) * 4;

    if (use_v_bound_)
    { // speed limit constraints
        // s_dot_i - v_bound_func_(s_i) <= 0.0
        nnz_jac_g += num_of_points_ * 2;
    }

    if (use_soft_safety_bound_)
    {
        // soft safety boundary constraints
        // s_i - soft_lower_s_i + lower_slack_i >= 0.0
        nnz_jac_g += num_of_points_ * 2;
        // s_i - soft_upper_s_i - upper_slack_i <= 0.0
        nnz_jac_g += num_of_points_ * 2;
    }

    // We have the full diagonal, and the first off-diagonal except for
    // the first and last variable
    nnz_h_lag = num_of_points_ * 5 - 1;

    // use the C style indexing (0-based) for the matrices
    index_style = IndexStyleEnum::C_STYLE;

    return true;
}

// returns the variable bounds
bool PiecewiseJerkSpeedNonlinearIpoptInterface::get_bounds_info(
    int n, double *x_l, double *x_u, int m, double *g_l, double *g_u)
{
    // default nlp_lower_bound_inf value in Ipopt
    double INF = 1.0e19;
    double LARGE_VELOCITY_VALUE = s_dot_max_;

    // bounds for variables
    // s
    x_l[0] = s_init_;
    x_u[0] = s_init_;
    // safety_bounds_就是s的硬约束
    for (int i = 1; i < num_of_points_; ++i)
    {
        x_l[i] = safety_bounds_[i].first;
        x_u[i] = safety_bounds_[i].second;
    }

    // s_dot
    // 变量的bounds不对速度做限制
    x_l[v_offset_] = s_dot_init_;
    x_u[v_offset_] = s_dot_init_;
    for (int i = 1; i < num_of_points_; ++i)
    {
        x_l[v_offset_ + i] = 0.0;
        x_u[v_offset_ + i] = LARGE_VELOCITY_VALUE;
    }

    // s_ddot
    x_l[a_offset_] = s_ddot_init_;
    x_u[a_offset_] = s_ddot_init_;
    // 是由参数确定的
    for (int i = 1; i < num_of_points_; ++i)
    {
        x_l[a_offset_ + i] = s_ddot_min_;
        x_u[a_offset_ + i] = s_ddot_max_;
    }

    if (use_soft_safety_bound_)
    {
        // lower_s_slack
        for (int i = 0; i < num_of_points_; ++i)
        {
            x_l[lower_s_slack_offset_ + i] = 0.0;
            x_u[lower_s_slack_offset_ + i] = INF;
        }

        // upper_s_slack
        for (int i = 0; i < num_of_points_; ++i)
        {
            x_l[upper_s_slack_offset_ + i] = 0.0;
            x_u[upper_s_slack_offset_ + i] = INF;
        }
    }

    // bounds for constraints
    // s monotone constraints s_i+1 - s_i
    for (int i = 0; i + 1 < num_of_points_; ++i)
    {
        g_l[i] = 0.0;
        g_u[i] = LARGE_VELOCITY_VALUE * delta_t_;
    }

    // jerk bound constraint
    // |s_ddot_i+1 - s_ddot_i| / delta_t <= s_dddot_max
    int offset = num_of_points_ - 1;
    for (int i = 0; i + 1 < num_of_points_; ++i)
    {
        g_l[offset + i] = s_dddot_min_;
        g_u[offset + i] = s_dddot_max_;
    }

    // position equality constraints,
    // s_i+1 - s_i - s_dot_i * delta_t - 1/3 * s_ddot_i * delta_t^2 - 1/6 *
    // s_ddot_i+1 * delta_t^2
    offset += num_of_points_ - 1;
    for (int i = 0; i + 1 < num_of_points_; ++i)
    {
        g_l[offset + i] = 0.0;
        g_u[offset + i] = 0.0;
    }

    // velocity equality constraints,
    // s_dot_i+1 - s_dot_i - 0.5 * delta_t * s_ddot_i - 0.5 * delta_t *
    // s_ddot_i+1
    offset += num_of_points_ - 1;
    for (int i = 0; i + 1 < num_of_points_; ++i)
    {
        g_l[offset + i] = 0.0;
        g_u[offset + i] = 0.0;
    }

    if (use_v_bound_)
    {
        // speed limit constraints
        // s_dot_i - v_bound_func_(s_i) <= 0.0
        offset += num_of_points_ - 1;
        for (int i = 0; i < num_of_points_; ++i)
        {
            g_l[offset + i] = -INF;
            g_u[offset + i] = 0.0;
        }
    }

    if (use_soft_safety_bound_)
    {
        // soft_s_bound constraints
        // s_i - soft_lower_s_i + lower_slack_i >= 0.0
        offset += num_of_points_;
        for (int i = 0; i < num_of_points_; ++i)
        {
            g_l[offset + i] = 0.0;
            g_u[offset + i] = INF;
        }

        // s_i - soft_upper_s_i - upper_slack_i <= 0.0
        offset += num_of_points_;
        for (int i = 0; i < num_of_points_; ++i)
        {
            g_l[offset + i] = -INF;
            g_u[offset + i] = 0.0;
        }
    }

    return true;
}

// returns the initial point for the problem
bool PiecewiseJerkSpeedNonlinearIpoptInterface::get_starting_point(
    int n, bool init_x, double *x, bool init_z, double *z_L, double *z_U, int m,
    bool init_lambda, double *lambda)
{
    if (!x_warm_start_.empty())
    {
        for (int i = 0; i < num_of_points_; ++i)
        {
            x[i] = x_warm_start_[i][0];
            x[v_offset_ + i] = x_warm_start_[i][1];
            x[a_offset_ + i] = x_warm_start_[i][2];
        }
    }

    if (use_soft_safety_bound_)
    {
        for (int i = 0; i < num_of_points_; ++i)
        {
            x[lower_s_slack_offset_ + i] = 0.0;
            x[upper_s_slack_offset_ + i] = 0.0;
        }
    }

    return true;

    // TODO(Jinyun): Implement better default warm start based on safety_bounds
    for (int i = 0; i < num_of_points_; ++i)
    {
        x[i] = std::min(5.0 * delta_t_ * i + s_init_, s_max_);
    }
    x[0] = s_init_;

    for (int i = 0; i < num_of_points_; ++i)
    {
        x[v_offset_ + i] = 5.0;
    }
    x[v_offset_] = s_dot_init_;

    for (int i = 0; i < num_of_points_; ++i)
    {
        x[a_offset_ + i] = 0.0;
    }
    x[a_offset_] = s_ddot_init_;

    if (use_soft_safety_bound_)
    {
        for (int i = 0; i < num_of_points_; ++i)
        {
            x[lower_s_slack_offset_ + i] = 0.0;
            x[upper_s_slack_offset_ + i] = 0.0;
        }
    }

    return true;
}

// returns the value of the objective function
// sum{i in 1..n} (x[i]-1)^2
bool PiecewiseJerkSpeedNonlinearIpoptInterface::eval_f(int n, const double *x,
                                                       bool new_x,
                                                       double &obj_value)
{
    obj_value = 0.0;
    // difference between ref spatial distace
    for (int i = 0; i < num_of_points_; ++i)
    {
        double s_diff = x[i] - s_ref_[i];
        obj_value += s_diff * s_diff * w_ref_s_;
    }

    // difference between ref speed
    for (int i = 0; i < num_of_points_; ++i)
    {
        double v_diff = x[v_offset_ + i] - v_ref_;
        obj_value += v_diff * v_diff * w_ref_v_;
    }

    // acceleration obj
    for (int i = 0; i < num_of_points_; ++i)
    {
        double a = x[a_offset_ + i];
        obj_value += a * a * w_overall_a_;
    }

    // jerk obj
    for (int i = 0; i + 1 < num_of_points_; ++i)
    {
        double j = (x[a_offset_ + i + 1] - x[a_offset_ + i]) / delta_t_;
        obj_value += j * j * w_overall_j_;
    }

    // centripetal acceleration obj
    for (int i = 0; i < num_of_points_; ++i)
    {
        double v = x[v_offset_ + i];
        double s = x[i];
        double kappa = curvature_curve_.Evaluate(0, s);
        double a_lat = v * v * kappa;
        obj_value += a_lat * a_lat * w_overall_centripetal_acc_;
    }

    if (has_end_state_target_)
    {
        double s_diff = x[num_of_points_ - 1] - s_target_;
        obj_value += s_diff * s_diff * w_target_s_;

        double v_diff = x[v_offset_ + num_of_points_ - 1] - v_target_;
        obj_value += v_diff * v_diff * w_target_v_;

        double a_diff = x[a_offset_ + num_of_points_ - 1] - a_target_;
        obj_value += a_diff * a_diff * w_target_a_;
    }

    if (use_soft_safety_bound_)
    {
        for (int i = 0; i < num_of_points_; ++i)
        {
            obj_value += x[lower_s_slack_offset_ + i] * w_soft_s_bound_;
            obj_value += x[upper_s_slack_offset_ + i] * w_soft_s_bound_;
        }
    }

    return true;
}

// return the gradient of the objective function grad_{x} f(x)
bool PiecewiseJerkSpeedNonlinearIpoptInterface::eval_grad_f(int n,
                                                            const double *x,
                                                            bool new_x,
                                                            double *grad_f)
{
    std::fill(grad_f, grad_f + n, 0.0);

    // ref. spatial distance objective
    for (int i = 0; i < num_of_points_; ++i)
    {
        auto s_diff = x[i] - s_ref_[i];
        grad_f[i] += 2.0 * s_diff * w_ref_s_;
    }

    // ref. speed objective
    for (int i = 0; i < num_of_points_; ++i)
    {
        auto v_diff = x[v_offset_ + i] - v_ref_;
        grad_f[v_offset_ + i] += 2.0 * v_diff * w_ref_v_;
    }

    // jerk objective
    double c = 2.0 / (delta_t_ * delta_t_) * w_overall_j_;
    grad_f[a_offset_] += -c * (x[a_offset_ + 1] - x[a_offset_]);
    for (int i = 1; i + 1 < num_of_points_; ++i)
    {
        grad_f[a_offset_ + i] += c * (2.0 * x[a_offset_ + i] -
                                      x[a_offset_ + i + 1] - x[a_offset_ + i - 1]);
    }
    grad_f[a_offset_ + num_of_points_ - 1] +=
        c *
        (x[a_offset_ + num_of_points_ - 1] - x[a_offset_ + num_of_points_ - 2]);

    // acceleration objective
    for (int i = 0; i < num_of_points_; ++i)
    {
        grad_f[a_offset_ + i] += 2.0 * x[a_offset_ + i] * w_overall_a_;
    }

    // centripetal acceleration objective
    for (int i = 0; i < num_of_points_; ++i)
    {
        double v = x[v_offset_ + i];
        double v2 = v * v;
        double v3 = v2 * v;
        double v4 = v3 * v;

        double s = x[i];
        double kappa = curvature_curve_.Evaluate(0, s);
        double kappa_dot = curvature_curve_.Evaluate(1, s);

        grad_f[i] += 2.0 * w_overall_centripetal_acc_ * v4 * kappa * kappa_dot;
        grad_f[v_offset_ + i] +=
            4.0 * w_overall_centripetal_acc_ * v3 * kappa * kappa;
    }

    if (has_end_state_target_)
    {
        double s_diff = x[num_of_points_ - 1] - s_target_;
        grad_f[num_of_points_ - 1] += 2.0 * s_diff * w_target_s_;

        double v_diff = x[v_offset_ + num_of_points_ - 1] - v_target_;
        grad_f[v_offset_ + num_of_points_ - 1] += 2.0 * v_diff * w_target_v_;

        double a_diff = x[a_offset_ + num_of_points_ - 1] - a_target_;
        grad_f[a_offset_ + num_of_points_ - 1] += 2.0 * a_diff * w_target_a_;
    }

    if (use_soft_safety_bound_)
    {
        for (int i = 0; i < num_of_points_; ++i)
        {
            grad_f[lower_s_slack_offset_ + i] += w_soft_s_bound_;
            grad_f[upper_s_slack_offset_ + i] += w_soft_s_bound_;
        }
    }

    return true;
}

// return the value of the constraints: g(x)
// (x[j+1]^2+1.5*x[j+1]-a[j])*cos(x[j+2]) - x[j] = 0
bool PiecewiseJerkSpeedNonlinearIpoptInterface::eval_g(int n, const double *x,
                                                       bool new_x, int m,
                                                       double *g)
{
    int offset = 0;

    // s monotone constraints s_i+1 - s_i
    for (int i = 0; i + 1 < num_of_points_; ++i)
    {
        g[i] = x[offset + i + 1] - x[i];
    }

    offset += num_of_points_ - 1;

    // jerk bound constraint, |s_ddot_i+1 - s_ddot_i| <= s_dddot_max
    // position equality constraints,
    // s_i+1 - s_i - s_dot_i * delta_t - 1/3 * s_ddot_i * delta_t^2 - 1/6 *
    // s_ddot_i+1 * delta_t^2
    // velocity equality constraints
    // s_dot_i+1 - s_dot_i - 0.5 * delta_t * (s_ddot_i + s_ddot_i+1)
    int coffset_jerk = offset;
    int coffset_position = coffset_jerk + num_of_points_ - 1;
    int coffset_velocity = coffset_position + num_of_points_ - 1;

    double t = delta_t_;
    double t2 = t * t;
    double t3 = t2 * t;

    for (int i = 0; i + 1 < num_of_points_; ++i)
    {
        double s0 = x[i];
        double s1 = x[i + 1];

        double v0 = x[v_offset_ + i];
        double v1 = x[v_offset_ + i + 1];

        double a0 = x[a_offset_ + i];
        double a1 = x[a_offset_ + i + 1];

        double j = (a1 - a0) / t;

        g[coffset_jerk + i] = j;
        g[coffset_position + i] =
            s1 - (s0 + v0 * t + 0.5 * a0 * t2 + 1.0 / 6.0 * j * t3);
        g[coffset_velocity + i] = v1 - (v0 + a0 * t + 0.5 * j * t2);
    }

    offset += 3 * (num_of_points_ - 1);

    if (use_v_bound_)
    {
        // speed limit constraints
        int coffset_speed_limit = offset;
        // s_dot_i - v_bound_func_(s_i) <= 0.0
        for (int i = 0; i < num_of_points_; ++i)
        {
            double s = x[i];
            double s_dot = x[v_offset_ + i];
            g[coffset_speed_limit + i] = s_dot - v_bound_func_.Evaluate(0, s);
        }

        offset += num_of_points_;
    }

    if (use_soft_safety_bound_)
    {
        // soft safety boundary constraints
        int coffset_soft_lower_s = offset;
        // s_i - soft_lower_s_i + lower_slack_i >= 0.0
        for (int i = 0; i < num_of_points_; ++i)
        {
            double s = x[i];
            double lower_s_slack = x[lower_s_slack_offset_ + i];
            g[coffset_soft_lower_s + i] =
                s - soft_safety_bounds_[i].first + lower_s_slack;
        }
        offset += num_of_points_;

        int coffset_soft_upper_s = offset;
        // s_i - soft_upper_s_i - upper_slack_i <= 0.0
        for (int i = 0; i < num_of_points_; ++i)
        {
            double s = x[i];
            double upper_s_slack = x[upper_s_slack_offset_ + i];
            g[coffset_soft_upper_s + i] =
                s - soft_safety_bounds_[i].second - upper_s_slack;
        }

        offset += num_of_points_;
    }

    return true;
}

// return the structure or values of the jacobian
bool PiecewiseJerkSpeedNonlinearIpoptInterface::eval_jac_g(
    int n, const double *x, bool new_x, int m, int nele_jac, int *iRow,
    int *jCol, double *values)
{
    if (values == nullptr)
    {
        int non_zero_index = 0;
        int constraint_index = 0;

        // s monotone constraints s_i+1 - s_i
        for (int i = 0; i + 1 < num_of_points_; ++i)
        {
            // s_i
            iRow[non_zero_index] = constraint_index;
            jCol[non_zero_index] = i;
            ++non_zero_index;

            // s_i+1
            iRow[non_zero_index] = constraint_index;
            jCol[non_zero_index] = i + 1;
            ++non_zero_index;

            ++constraint_index;
        }

        // jerk bound constraint, |s_ddot_i+1 - s_ddot_i| / delta_t <= s_dddot_max
        for (int i = 0; i + 1 < num_of_points_; ++i)
        {
            // a_i
            iRow[non_zero_index] = constraint_index;
            jCol[non_zero_index] = a_offset_ + i;
            ++non_zero_index;

            // a_i+1
            iRow[non_zero_index] = constraint_index;
            jCol[non_zero_index] = a_offset_ + i + 1;
            ++non_zero_index;

            ++constraint_index;
        }

        // position equality constraints
        for (int i = 0; i + 1 < num_of_points_; ++i)
        {
            // s_i
            iRow[non_zero_index] = constraint_index;
            jCol[non_zero_index] = i;
            ++non_zero_index;

            // v_i
            iRow[non_zero_index] = constraint_index;
            jCol[non_zero_index] = v_offset_ + i;
            ++non_zero_index;

            // a_i
            iRow[non_zero_index] = constraint_index;
            jCol[non_zero_index] = a_offset_ + i;
            ++non_zero_index;

            // s_i+1
            iRow[non_zero_index] = constraint_index;
            jCol[non_zero_index] = i + 1;
            ++non_zero_index;

            // a_i+1
            iRow[non_zero_index] = constraint_index;
            jCol[non_zero_index] = a_offset_ + i + 1;
            ++non_zero_index;

            ++constraint_index;
        }

        // velocity equality constraints
        for (int i = 0; i + 1 < num_of_points_; ++i)
        {
            // v_i
            iRow[non_zero_index] = constraint_index;
            jCol[non_zero_index] = v_offset_ + i;
            ++non_zero_index;

            // a_i
            iRow[non_zero_index] = constraint_index;
            jCol[non_zero_index] = a_offset_ + i;
            ++non_zero_index;

            // v_i+1
            iRow[non_zero_index] = constraint_index;
            jCol[non_zero_index] = v_offset_ + i + 1;
            ++non_zero_index;

            // a_i+1
            iRow[non_zero_index] = constraint_index;
            jCol[non_zero_index] = a_offset_ + i + 1;
            ++non_zero_index;

            ++constraint_index;
        }

        if (use_v_bound_)
        {
            // speed limit constraints
            // s_dot_i - v_bound_func_(s_i) <= 0.0
            for (int i = 0; i < num_of_points_; ++i)
            {
                // s_i
                iRow[non_zero_index] = constraint_index;
                jCol[non_zero_index] = i;
                ++non_zero_index;

                // v_i
                iRow[non_zero_index] = constraint_index;
                jCol[non_zero_index] = v_offset_ + i;
                ++non_zero_index;

                ++constraint_index;
            }
        }

        if (use_soft_safety_bound_)
        {
            // soft_s_bound constraints
            // s_i - soft_lower_s_i + lower_slack_i >= 0.0
            for (int i = 0; i < num_of_points_; ++i)
            {
                // s_i
                iRow[non_zero_index] = constraint_index;
                jCol[non_zero_index] = i;
                ++non_zero_index;

                // lower_slack_i
                iRow[non_zero_index] = constraint_index;
                jCol[non_zero_index] = lower_s_slack_offset_ + i;
                ++non_zero_index;

                ++constraint_index;
            }
            // s_i - soft_upper_s_i - upper_slack_i <= 0.0

            for (int i = 0; i < num_of_points_; ++i)
            {
                // s_i
                iRow[non_zero_index] = constraint_index;
                jCol[non_zero_index] = i;
                ++non_zero_index;

                // upper_slack_i
                iRow[non_zero_index] = constraint_index;
                jCol[non_zero_index] = upper_s_slack_offset_ + i;
                ++non_zero_index;

                ++constraint_index;
            }
        }
    }
    else
    {
        int non_zero_index = 0;
        // s monotone constraints s_i+1 - s_i
        for (int i = 0; i + 1 < num_of_points_; ++i)
        {
            // s_i
            values[non_zero_index] = -1.0;
            ++non_zero_index;

            // s_i+1
            values[non_zero_index] = 1.0;
            ++non_zero_index;
        }

        // jerk bound constraint, |s_ddot_i+1 - s_ddot_i| / delta_t <= s_dddot_max
        for (int i = 0; i + 1 < num_of_points_; ++i)
        {
            // a_i
            values[non_zero_index] = -1.0 / delta_t_;
            ++non_zero_index;

            // a_i+1
            values[non_zero_index] = 1.0 / delta_t_;
            ++non_zero_index;
        }

        // position equality constraints
        for (int i = 0; i + 1 < num_of_points_; ++i)
        {
            // s_i
            values[non_zero_index] = -1.0;
            ++non_zero_index;

            // v_i
            values[non_zero_index] = -delta_t_;
            ++non_zero_index;

            // a_i
            values[non_zero_index] = -1.0 / 3.0 * delta_t_ * delta_t_;
            ++non_zero_index;

            // s_i+1
            values[non_zero_index] = 1.0;
            ++non_zero_index;

            // a_i+1
            values[non_zero_index] = -1.0 / 6.0 * delta_t_ * delta_t_;
            ++non_zero_index;
        }

        // velocity equality constraints
        for (int i = 0; i + 1 < num_of_points_; ++i)
        {
            // v_i
            values[non_zero_index] = -1.0;
            ++non_zero_index;

            // a_i
            values[non_zero_index] = -0.5 * delta_t_;
            ++non_zero_index;

            // v_i+1
            values[non_zero_index] = 1.0;
            ++non_zero_index;

            // a_i+1
            values[non_zero_index] = -0.5 * delta_t_;
            ++non_zero_index;
        }

        if (use_v_bound_)
        {
            // speed limit constraints
            // s_dot_i - v_bound_func_(s_i) <= 0.0
            for (int i = 0; i < num_of_points_; ++i)
            {
                // s_i
                double s = x[i];
                values[non_zero_index] = -1.0 * v_bound_func_.Evaluate(1, s);
                ++non_zero_index;

                // v_i
                values[non_zero_index] = 1.0;
                ++non_zero_index;
            }
        }

        if (use_soft_safety_bound_)
        {
            // soft_s_bound constraints
            // s_i - soft_lower_s_i + lower_slack_i >= 0.0
            for (int i = 0; i < num_of_points_; ++i)
            {
                // s_i
                values[non_zero_index] = 1.0;
                ++non_zero_index;

                // lower_slack_i
                values[non_zero_index] = 1.0;
                ++non_zero_index;
            }

            // s_i - soft_upper_s_i - upper_slack_i <= 0.0
            for (int i = 0; i < num_of_points_; ++i)
            {
                // s_i
                values[non_zero_index] = 1.0;
                ++non_zero_index;

                // upper_slack_i
                values[non_zero_index] = -1.0;
                ++non_zero_index;
            }
        }
    }
    return true;
}

// return the structure or values of the hessian
bool PiecewiseJerkSpeedNonlinearIpoptInterface::eval_h(
    int n, const double *x, bool new_x, double obj_factor, int m,
    const double *lambda, bool new_lambda, int nele_hess, int *iRow, int *jCol,
    double *values)
{
    if (values == nullptr)
    {
        int nz_index = 0;
        for (int i = 0; i < num_of_points_; ++i)
        {
            iRow[nz_index] = i;
            jCol[nz_index] = i;
            ++nz_index;
        }

        for (int i = 0; i < num_of_points_; ++i)
        {
            iRow[nz_index] = i;
            jCol[nz_index] = v_offset_ + i;
            ++nz_index;
        }

        for (int i = 0; i < num_of_points_; ++i)
        {
            iRow[nz_index] = v_offset_ + i;
            jCol[nz_index] = v_offset_ + i;
            ++nz_index;
        }

        for (int i = 0; i < num_of_points_; ++i)
        {
            iRow[nz_index] = a_offset_ + i;
            jCol[nz_index] = a_offset_ + i;
            ++nz_index;
        }

        for (int i = 0; i + 1 < num_of_points_; ++i)
        {
            iRow[nz_index] = a_offset_ + i;
            jCol[nz_index] = a_offset_ + i + 1;
            ++nz_index;
        }

        for (int i = 0; i < nz_index; ++i)
        {
            int r = iRow[i];
            int c = jCol[i];
            hessian_mapper_[to_hash_key(r, c)] = i;
        }
    }
    else
    {
        std::fill(values, values + nele_hess, 0.0);

        // speed by curvature objective
        for (int i = 0; i < num_of_points_; ++i)
        {
            auto s_index = i;
            auto v_index = v_offset_ + i;

            auto s = x[s_index];
            auto v = x[v_index];

            auto kappa = curvature_curve_.Evaluate(0, s);
            auto kappa_dot = curvature_curve_.Evaluate(1, s);
            auto kappa_ddot = curvature_curve_.Evaluate(2, s);

            auto v2 = v * v;
            auto v3 = v2 * v;
            auto v4 = v3 * v;

            auto h_s_s_obj =
                2.0 * kappa_dot * kappa_dot * v4 + 2.0 * kappa * kappa_ddot * v4;
            auto h_s_s_index = hessian_mapper_[to_hash_key(s_index, s_index)];
            values[h_s_s_index] +=
                h_s_s_obj * w_overall_centripetal_acc_ * obj_factor;

            auto h_s_v_obj = 8.0 * kappa * kappa_dot * v3;
            auto h_s_v_index = hessian_mapper_[to_hash_key(s_index, v_index)];
            values[h_s_v_index] +=
                h_s_v_obj * w_overall_centripetal_acc_ * obj_factor;

            auto h_v_v_obj = 12.0 * kappa * kappa * v2;
            auto h_v_v_index = hessian_mapper_[to_hash_key(v_index, v_index)];
            values[h_v_v_index] +=
                h_v_v_obj * w_overall_centripetal_acc_ * obj_factor;
        }

        // spatial distance reference objective
        for (int i = 0; i < num_of_points_; ++i)
        {
            auto h_s_s_index = hessian_mapper_[to_hash_key(i, i)];
            values[h_s_s_index] += 2.0 * w_ref_s_ * obj_factor;
        }

        // speed limit constraint
        if (use_v_bound_)
        {
            int lambda_offset = 4 * (num_of_points_ - 1);
            for (int i = 0; i < num_of_points_; ++i)
            {
                auto s_index = i;

                auto s = x[s_index];

                auto v_bound_ddot = v_bound_func_.Evaluate(2, s);

                auto h_s_s_constr = -1.0 * v_bound_ddot;
                auto h_s_s_index = hessian_mapper_[to_hash_key(s_index, s_index)];
                values[h_s_s_index] += h_s_s_constr * lambda[lambda_offset + i];
            }
        }

        for (int i = 0; i < num_of_points_; ++i)
        {
            auto a_index = a_offset_ + i;
            auto h_index = hessian_mapper_[to_hash_key(a_index, a_index)];
            values[h_index] += 2.0 * w_overall_a_ * obj_factor;
        }

        auto c = 2.0 / delta_t_ / delta_t_ * w_overall_j_ * obj_factor;
        for (int i = 0; i + 1 < num_of_points_; ++i)
        {
            auto a0_index = a_offset_ + i;
            auto a1_index = a_offset_ + i + 1;

            auto h_a0_a0_index = hessian_mapper_[to_hash_key(a0_index, a0_index)];
            values[h_a0_a0_index] += c;

            auto h_a0_a1_index = hessian_mapper_[to_hash_key(a0_index, a1_index)];
            values[h_a0_a1_index] += -c;

            auto h_a1_a1_index = hessian_mapper_[to_hash_key(a1_index, a1_index)];
            values[h_a1_a1_index] += c;
        }

        for (int i = 0; i < num_of_points_; ++i)
        {
            auto v_index = i + v_offset_;
            auto key = to_hash_key(v_index, v_index);
            auto index = hessian_mapper_[key];
            values[index] += 2.0 * w_ref_v_ * obj_factor;
        }

        if (has_end_state_target_)
        {
            // target s
            auto s_end_index = num_of_points_ - 1;
            auto s_key = to_hash_key(s_end_index, s_end_index);

            auto s_index = hessian_mapper_[s_key];

            values[s_index] += 2.0 * w_target_s_ * obj_factor;

            // target v
            auto v_end_index = 2 * num_of_points_ - 1;
            auto v_key = to_hash_key(v_end_index, v_end_index);

            auto v_index = hessian_mapper_[v_key];

            values[v_index] += 2.0 * w_target_v_ * obj_factor;

            // target a
            auto a_end_index = 3 * num_of_points_ - 1;
            auto a_key = to_hash_key(a_end_index, a_end_index);

            auto a_index = hessian_mapper_[a_key];

            values[a_index] += 2.0 * w_target_a_ * obj_factor;
        }
    }
    return true;
}

void PiecewiseJerkSpeedNonlinearIpoptInterface::get_optimization_results(
    std::vector<double> *ptr_opt_s, std::vector<double> *ptr_opt_v,
    std::vector<double> *ptr_opt_a)
{
    *ptr_opt_s = opt_s_;
    *ptr_opt_v = opt_v_;
    *ptr_opt_a = opt_a_;
}

void PiecewiseJerkSpeedNonlinearIpoptInterface::finalize_solution(
    Ipopt::SolverReturn status, int n, const double *x, const double *z_L,
    const double *z_U, int m, const double *g, const double *lambda,
    double obj_value, const Ipopt::IpoptData *ip_data,
    Ipopt::IpoptCalculatedQuantities *ip_cq)
{
    opt_s_.clear();
    opt_v_.clear();
    opt_a_.clear();

    for (int i = 0; i < num_of_points_; ++i)
    {
        double s = x[i];
        double v = x[v_offset_ + i];
        double a = x[a_offset_ + i];

        opt_s_.push_back(s);
        opt_v_.push_back(v);
        opt_a_.push_back(a);
    }

    if (use_soft_safety_bound_)
    {
        // statistic analysis on soft bound intrusion by inspecting slack variable
        double lower_s_mean_intrusion = 0.0;
        double lower_s_highest_intrusion = -std::numeric_limits<double>::infinity();
        int lower_s_highest_intrustion_index = 0.0;
        double upper_s_mean_intrusion = 0.0;
        double upper_s_highest_intrusion = -std::numeric_limits<double>::infinity();
        int upper_s_highest_intrustion_index = 0.0;

        for (int i = 0; i < num_of_points_; ++i)
        {
            double lower_s_slack = x[lower_s_slack_offset_ + i];
            double upper_s_slack = x[upper_s_slack_offset_ + i];

            lower_s_mean_intrusion += lower_s_slack;
            upper_s_mean_intrusion += upper_s_slack;

            if (lower_s_highest_intrusion < lower_s_slack)
            {
                lower_s_highest_intrusion = lower_s_slack;
                lower_s_highest_intrustion_index = i;
            }

            if (upper_s_highest_intrusion < upper_s_slack)
            {
                upper_s_highest_intrusion = upper_s_slack;
                upper_s_highest_intrustion_index = i;
            }
        }

        lower_s_mean_intrusion /= static_cast<double>(num_of_points_);
        upper_s_mean_intrusion /= static_cast<double>(num_of_points_);

        LOG(INFO) << "lower soft s boundary average intrustion is ["
                  << lower_s_mean_intrusion << "] with highest value of ["
                  << lower_s_highest_intrusion << "] at time ["
                  << delta_t_ * static_cast<double>(lower_s_highest_intrustion_index)
                  << "].";
        LOG(INFO) << "upper soft s boundary average intrustion is ["
                  << upper_s_mean_intrusion << "] with highest value of ["
                  << upper_s_highest_intrusion << "] at time ["
                  << delta_t_ * static_cast<double>(upper_s_highest_intrustion_index)
                  << "].";
    }
}

void PiecewiseJerkSpeedNonlinearIpoptInterface::set_curvature_curve(
    const PiecewiseJerkTrajectory1d &curvature_curve)
{
    curvature_curve_ = curvature_curve;
}

void PiecewiseJerkSpeedNonlinearIpoptInterface::set_speed_limit_curve(
    const PiecewiseJerkTrajectory1d &v_bound_f)
{
    v_bound_func_ = v_bound_f;
    use_v_bound_ = true;
}

void PiecewiseJerkSpeedNonlinearIpoptInterface::set_reference_speed(
    const double v_ref)
{
    v_ref_ = v_ref;
}

void PiecewiseJerkSpeedNonlinearIpoptInterface::set_safety_bounds(
    const std::vector<std::pair<double, double>> &safety_bounds)
{
    safety_bounds_ = safety_bounds;
}

void PiecewiseJerkSpeedNonlinearIpoptInterface::set_soft_safety_bounds(
    const std::vector<std::pair<double, double>> &soft_safety_bounds)
{
    soft_safety_bounds_ = soft_safety_bounds;
    use_soft_safety_bound_ = true;
}

int PiecewiseJerkSpeedNonlinearIpoptInterface::to_hash_key(const int i,
                                                           const int j) const
{
    return i * num_of_variables_ + j;
}

void PiecewiseJerkSpeedNonlinearIpoptInterface::set_end_state_target(
    const double s_target, const double v_target, const double a_target)
{
    s_target_ = s_target;
    v_target_ = v_target;
    a_target_ = a_target;
    has_end_state_target_ = true;
}

void PiecewiseJerkSpeedNonlinearIpoptInterface::set_w_target_state(
    const double w_target_s, const double w_target_v, const double w_target_a)
{
    w_target_s_ = w_target_s;
    w_target_v_ = w_target_v;
    w_target_a_ = w_target_a;
}

void PiecewiseJerkSpeedNonlinearIpoptInterface::set_w_overall_a(
    const double w_overall_a)
{
    w_overall_a_ = w_overall_a;
}

void PiecewiseJerkSpeedNonlinearIpoptInterface::set_w_overall_j(
    const double w_overall_j)
{
    w_overall_j_ = w_overall_j;
}

void PiecewiseJerkSpeedNonlinearIpoptInterface::set_w_overall_centripetal_acc(
    const double w_overall_centripetal_acc)
{
    w_overall_centripetal_acc_ = w_overall_centripetal_acc;
}

void PiecewiseJerkSpeedNonlinearIpoptInterface::set_w_reference_speed(
    const double w_reference_speed)
{
    w_ref_v_ = w_reference_speed;
}

void PiecewiseJerkSpeedNonlinearIpoptInterface::
    set_w_reference_spatial_distance(const double w_ref_s)
{
    w_ref_s_ = w_ref_s;
}

void PiecewiseJerkSpeedNonlinearIpoptInterface::set_w_soft_s_bound(
    const double w_soft_s_bound)
{
    w_soft_s_bound_ = w_soft_s_bound;
}

void PiecewiseJerkSpeedNonlinearIpoptInterface::set_warm_start(
    const std::vector<std::vector<double>> &speed_profile)
{
    x_warm_start_ = speed_profile;
}

void PiecewiseJerkSpeedNonlinearIpoptInterface::set_reference_spatial_distance(
    const std::vector<double> &s_ref)
{
    s_ref_ = s_ref;
}